#include "../include/hough.h"

#include <math.h>
#include <iostream>

using namespace std;
///############################################################# CONSTRUCTEURS / DESTRUCTEURS

Hough::Hough(const int _largeur, const int _hauteur): largeur(_largeur), hauteur(_hauteur)
{
    maxRho = sqrt(pow(largeur,2) + pow(hauteur,2));
    maxIndiceTheta = 180;
    maxIndiceRho = (int)(1 + maxRho);

    vector<int> r;
    r.assign(maxIndiceTheta,0);
    accumulateur.assign(maxIndiceRho,r);
}

Hough::~Hough()
{}

///############################################################# ACCESSEURS


///############################################################# MUTATEURS


void Hough::vote(int x,int y)
{

    for(int indiceTheta = 0; indiceTheta < maxIndiceTheta; indiceTheta++)
    {
        // calcul de l'angle
        double theta = ((double)indiceTheta/maxIndiceTheta)*M_PI;

        // calcul de rho
        double rho = y*cos(theta) + x*sin(theta);
        rho = sqrt(rho*rho);

        accumulateur[int(rho)][indiceTheta]++;

    }
}

vector<double> Hough::gagnant()
{
    vector<double> retour;

    double max=0, fenRho=0, fenTheta=0;


    for(int indiceRho = 0; indiceRho < maxIndiceRho; indiceRho++)
    {
        for(int indiceTheta = 0; indiceTheta < maxIndiceTheta; indiceTheta++)
        {
            if (accumulateur[indiceRho][indiceTheta] >= max)
            {
                max = accumulateur[indiceRho][indiceTheta];
                fenRho   = indiceRho;
                fenTheta = indiceTheta;
            }
        }
    }

    accumulateur[fenRho][fenTheta] = 0;


    double rho = ((double)fenRho/maxIndiceRho) * maxRho;
    double theta = ((double)fenTheta/maxIndiceTheta) * M_PI;

    cout << "Rho: " << rho << " fenRho: " << fenRho << endl;
    cout << "Theta: " << theta << " fenTheta: " << fenTheta << endl;

    retour.push_back(rho);
    retour.push_back(theta);

    return retour;
}


vector<double> Hough::rhoThetaToAb(const double rho, const double theta)
{
    vector<double> retour;
//
    double a = 0, b = 0;
    a = rho * cos(theta);
    b = rho * sin(theta);

    retour.push_back(a);
    retour.push_back(b);

    return retour;
}



